#define left_pin    A0//左侧模拟灰度传感器
#define right_pin   A2//右侧模拟灰度传感器
#define LFwheel_1   9//左前轮引脚1
#define LFwheel_2   10//左前轮引脚2
#define RFwheel_1   5//右前轮引脚1
#define RFwheel_2   6//右前轮引脚2

#define after_pin   A4//右侧模拟灰度传感器

//P、I、D 参数需要根据具体情况进行设置
float kp=1;
float ki=0.5;
float kd=0.5;
int left_val=0;
int right_val=0;
int lastError=0;
int allError=0;
int v_st=80;
int v_turn=0;
void setup() {
  //pinMode(A0, INPUT_PULLUP);
  //pinMode(A1, INPUT_PULLUP);
  Serial.begin(9600);
}

void loop() {
//获取两个模拟灰度传感器的数值
  left_val=analogRead(left_pin);
  right_val=analogRead(right_pin);
    after_val=analogRead(after_pin);
  //调用PID调节函数
  //PID(left_val,right_val);
  //调用PD调节函数
  //PD(left_val,right_val);
  //调用P调节函数
  P(left_val,right_val);
  //调用运动函数
      //Serial.println(v_turn);
  sport(v_turn);
  Serial.print(left_val);
  Serial.print(" ");
  Serial.println(right_val);


  delay(100);
}
//PID调节函数
void PID(int left_val,int right_val){
  int Error=left_val-right_val;
  allError+=Error;
  float p=kp*Error;
  float i=ki*allError;
  float d=kd*(Error-lastError);
  lastError=Error;
  v_turn=int(p+i+d);
}
//P调节函数
void P(int left_val,int right_val){
  int Error=left_val-right_val;
  float p=kp*Error;
  v_turn=int(p);

}
//PD调节函数
void PD(int left_val,int right_val){
  int Error=left_val-right_val;
  float p=kp*Error;
  float d=kd*(Error-lastError);
  lastError=Error;
  v_turn=int(p+d);
}
//运动函数
void sport(int v_turn){
  int speed_left=v_st+v_turn;
  int speed_right=v_st-v_turn;

//  if(speed_left > 255){
//    speed_left = 255;
//  }
//    if(speed_left < 50){
//    speed_left = 50;
//  }
//    if(speed_right > 255){
//    speed_left = 255;
//  }
//    if(speed_right < 50){
//    speed_left = 50;
//  }

//  Serial.print(speed_left);
//  Serial.print(" ");
//  Serial.println(speed_right);

  //左前轮
  //analogWrite(LFpwm,speed_left);
  analogWrite(LFwheel_1,speed_left);
  analogWrite(LFwheel_2,0);
  
  //右前轮
  //analogWrite(RFpwm,speed_right);
  analogWrite(RFwheel_1,speed_right);
  analogWrite(RFwheel_2,0);

  //左后轮
  analogWrite(LBpwm,speed_left);
  digitalWrite(LBwheel_1,LOW);
  digitalWrite(LBwheel_2,HIGH);
  
  //右后轮
  analogWrite(RBpwm,speed_right);
  digitalWrite(RBwheel_1,LOW);
  digitalWrite(RBwheel_2,HIGH);
}
